#ifndef ANDRES_SIGN_POLE_MATCHING_UTILS_H
#define ANDRES_SIGN_POLE_MATCHING_UTILS_H

#include <opencv2/opencv.hpp>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Core>
#include <vector>
#include <string>

#include <base/defines.h>
#include <base/camera.h>
#include <base/common.h>

namespace SPM
{
    class utils
    {
    public:

        static bool Matching(const DlFrame &dl,
			const hdmap_frame& hdmap_f,
            const Camera &cam,
			const Pose &pose,
            std::vector<std::pair<Eigen::Vector2d, Eigen::Vector3d>>& match_results);


        static bool Translate3d_2d(const std::vector<std::pair<Eigen::Vector2d, Eigen::Vector3d>>& pole_sign_matches,  
            const Camera& camera,
            Eigen::Matrix3d& R,
            Eigen::Vector3d& T, 
            double& rmse);

        static bool ComputeRT(const std::vector<std::pair<DlSign, hdmap_sign>>& track_results,
			const Pose &pose,
            const Camera &cam,
            Eigen::Matrix3d& R,
            Eigen::Vector3d& T, 
            double& rmse,
            double tolerance,
            std::vector<std::pair<Eigen::Vector2d, Eigen::Vector3d>>& match_pairs);        
    };
}

#endif